
#include "genki_web_plotclock.h"

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <hi_time.h>
#include <float.h>
#include "utils_file.h"
#include "kinematics.h"
#include "cJSON.h"
#include <math.h>
#include <unistd.h>

#define MAX(x, y) (((x) > (y)) ? (x) : (y))
#define MIN(x, y) (((x) < (y)) ? (x) : (y))

double servo_range_left[] = {-12, 204};
double scale_ratio_left = 190.0f / 180.0f;
double servo_range_right[] = {-12, 202};
double scale_ratio_right = 185.0f / 180.0f;


static int transform_angle_to_servo(double angle, const double *range, double scale_ratio) {
    double servo_center = (range[0] + range[1]) * 0.5f;
    double servo_angle = servo_center + (angle - 90) * scale_ratio;

    // 返回四舍五入后的servo_angle
    return (int) (servo_angle + 0.5f);
}

//对端口和角度进行限制
static int servo_set_angle(int port, int angle) {
    if (port < 0 || port > 15) {
        return -1;
    }

    if (angle < -20 || angle > 210) {
        return -1;
    }

    pca9685_servo_set_angle(port, angle);

    pre_angle_arr[port] = angle;

    return 0;
}

//返回当前端口的舵机角度
static int servo_get_angle(int port){
    return pre_angle_arr[port];
}

static int transform_angle_left(double angle) {
    return transform_angle_to_servo(LEFT_START_ANGLE + angle, servo_range_left, scale_ratio_left);
}
static int transform_angle_right(double angle) {
    return transform_angle_to_servo(RIGHT_START_ANGLE + angle, servo_range_right, scale_ratio_right);
}

static void save_html(const char *page, const char *html) {
    int fd = UtilsFileOpen(page, O_CREAT_FS | O_TRUNC_FS | O_WRONLY_FS, 0);

    if (fd > 0) {
        UtilsFileWrite(fd, html, strlen(html));
        UtilsFileClose(fd);
    }
}

static int doHtml(genki_web_request_t *request, genki_web_response_t *response) {
    if (request->method != GET) {
        return -1;
    }
    const char *page = "plot.html";
    unsigned int size;
    char buf[128];
    if (UtilsFileStat(page, &size) >= 0) {
        sprintf(buf, "%d", size);
        response->setHeader(response, "Content-Length", buf);

        int fd = UtilsFileOpen(page, O_RDONLY_FS, 0);
        if (fd > 0) {
            unsigned char num;
            while ((num = UtilsFileRead(fd, buf, 128)) > 0) {
                response->write(response, buf, num);
            }
            UtilsFileClose(fd);
        } else {
            sprintf(buf, "<html><body><h2 id='a'>ERROR</h2></body></html>");
            response->write(response, buf, strlen(buf));
        }
    } else {
        const char *html = "<!DOCTYPE html><html lang=\"en\"><head><meta charset=\"UTF-8\"><title>传智教育元气派</title><style> body {user-select: none;text-align: center;}  table {margin: 0 auto}  td div {width: 150px;height: 150px;border: 1px solid darkgrey;display: flex;align-items: center;justify-content: center;color: black;}  #s {background-color: red;color: white;}  td div:active {background: deepskyblue;color: white;}  button {margin: 0.5rem;width: 9rem;height: 3rem;font-size: 1.2rem;border-radius: 1rem;}  button:hover {border-radius: 1rem;background-color: deepskyblue;color: aliceblue;}</style></head><body><h1>小贱钟控制</h1><button id=\"a\">安装角度</button><br><button id=\"d\">画圆</button><br><br>端口:<input id=\"k\" value=\"0\">角度:<input id=\"l\" value=\"0\"><button id=\"b\">测试舵机端口</button><br><br>坐标X:<input id=\"m\" value=\"0\">坐标Y:<input id=\"n\" value=\"0\"><button id=\"c\">反解</button><br><br><br><br>关节角列表:<input id=\"j\" value=\"[]\"><button id=\"r2\">运行</button><br><br>路径点列表:<input id=\"g\" value=\"[]\"><button id=\"r1\">运行</button><br><script> function u(p) {return `${p}`;}function g(i) {return document.getElementById(i);}function fe(e, f) {e.addEventListener('click', function (e) {/* do something*/f();});}function fg(u, f) {let x = new XMLHttpRequest();x.onreadystatechange = function () {if (this.readyState === 4 && this.status === 200) {f(x);}};x.open(\"GET\", u, true);x.send();}function fgp(u, f, d) {let x = new XMLHttpRequest();x.open(\"POST\", u, true);x.setRequestHeader(\"content-Type\",\"application/json; charset=utf-8\");x.onreadystatechange = function () {if (this.readyState === 4 && this.status === 200) {f(x);}};x.send(typeof(d)=='string' ? d : JSON.stringify(d));}fe(g('a'), () => {fg(u('/plot/init'), () => {});});fe(g('b'), () => {let p = g('k').value;let a = g('l').value;fg(u(`/plot/test?p=${p}&a=${a}`), () => {});});fe(g('c'), () => {let p = g('m').value;let a = g('n').value;fg(u(`/plot/inverse?x=${p}&y=${a}`), (x) => {console.log(x);});});fe(g('r1'), () => {fgp(u(`/plot/path`), (x) => {console.log(x);}, g('g').value);});fe(g('r2'), () => {fgp(u(`/plot/joints`), (x) => {console.log(x);}, g('j').value);});fe(g('d'), () => {fg(u('/plot/circle'), () => {});});</script></body></html>";
        size_t len = strlen(html);

        char buf[128];
        sprintf(buf, "%d", len);
        response->setHeader(response, "Content-Type", "text/html; charset=UTF-8");
        response->setHeader(response, "Content-Length", buf);
        response->write(response, html, len);
        save_html(page, html);
    }

    return 1;
}

static int doTest(genki_web_request_t *request, genki_web_response_t *response) {

    char *p1 = strtok(request->queryString, "&");
    if (p1) {
        char *p2 = strtok(NULL, "");

        char *key1 = strtok(p1, "=");
        char *value1 = strtok(NULL, "");

        char *key2 = strtok(p2, "=");
        char *value2 = strtok(NULL, "");

        char str_port[64] = {0};
        char str_angle[64] = {0};
        unsigned char flag = 1;
        if (strcasecmp(key1, "p") == 0 && strcasecmp(key2, "a") == 0) {
            memcpy(str_port, value1, strlen(value1));
            memcpy(str_angle, value2, strlen(value2));
        } else {
            flag = 0;
        }

        if (flag) {
            int port;
            int angle;
            sscanf(str_port, "%d", &port);
            sscanf(str_angle, "%d", &angle);

            servo_set_angle(port, angle);
        }
    }
    return 1;
}

static int doInverse(genki_web_request_t *request, genki_web_response_t *response) {

    char *p1 = strtok(request->queryString, "&");
    if (p1) {
        char *p2 = strtok(NULL, "");

        char *key1 = strtok(p1, "=");
        char *value1 = strtok(NULL, "");

        char *key2 = strtok(p2, "=");
        char *value2 = strtok(NULL, "");

        char str_x[64] = {0};
        char str_y[64] = {0};
        unsigned char flag = 1;
        if (strcasecmp(key1, "x") == 0 && strcasecmp(key2, "y") == 0) {
            memcpy(str_x, value1, strlen(value1));
            memcpy(str_y, value2, strlen(value2));
        } else if (strcasecmp(key2, "x") == 0 && strcasecmp(key1, "y") == 0) {
            memcpy(str_x, value2, strlen(value2));
            memcpy(str_y, value1, strlen(value1));
        } else {
            flag = 0;
        }

        if (flag) {
            int x;
            int y;
            sscanf(str_x, "%d", &x);
            sscanf(str_y, "%d", &y);

            printf("x: %d y: %d \r\n", x, y);
            struct Point_t point;
            point.x = x;
            point.y = y;
            double *angles = inverse(point);

            if (angles) {
                printf("inverse success α: %f β: %f \r\n", angles[0], angles[1]);
                platform_up();

                update_serve_angle(angles[0], angles[1]);
                free(angles);

                platform_down();
                platform_init();
            } else {
                printf("inverse failed x: %d y: %d\r\n", x, y);
                platform_shake();
            }
        }
    }
    return 1;
}


static int update_serve_angle(double alpha, double beta) {

    int angle_left_target = transform_angle_left(alpha);
    int angle_right_target = transform_angle_right(beta);

    // 获取当前的舵机角度
    double angle_left_cur = (double)servo_get_angle(SERVO_LEFT_PORT);
    double angle_right_cur = (double)servo_get_angle(SERVO_RIGHT_PORT);

    if ((int)angle_left_cur == ANGLE_NOT_INIT && (int)angle_right_cur == ANGLE_NOT_INIT) {
        servo_set_angle(SERVO_LEFT_PORT, angle_left_target);
        servo_set_angle(SERVO_RIGHT_PORT, angle_left_target);
        return 1;
    }

    double speed = 100; // 100度/秒
    double diff_left = angle_left_target - angle_left_cur;
    double diff_right = angle_right_target - angle_right_cur;
    double max_angle_diff = MAX(fabs(diff_left), fabs(diff_right));
    int n = MAX((int) (max_angle_diff * 0.5f + 0.5f), 2);

    double angle_left_step = diff_left / n;
    double angle_right_step = diff_right / n;
    double time_sleep_step = max_angle_diff / (speed * n);

    printf("alpha: %f, beta: %f -> left: %d, right: %d n: %d sleep: %d \n", alpha, beta,
           angle_left_target, angle_right_target, n, (int)(time_sleep_step * 1000 * 1000));
    // 将 角度分成 n 等份进行细分控制
    for (int i = 0; i < n; i++) {
        angle_left_cur += angle_left_step;
        angle_right_cur += angle_right_step;

        servo_set_angle(SERVO_LEFT_PORT, (int)(angle_left_cur + 0.5f));
        servo_set_angle(SERVO_RIGHT_PORT, (int)(angle_right_cur + 0.5f));

        usleep((int)(time_sleep_step * 1000 * 1000));
    }
//    for (int i = 0; i < 4; ++i) {
//        servo_set_angle(i, angle_left_target);
//        servo_set_angle(i + 4, angle_right_target);
//    }

    return 0;
}


static int doCircle(genki_web_request_t *request, genki_web_response_t *response) {

    platform_up();

    struct Point_t center;
    center.x = 0.0;
    center.y = 60.0;
    double radius = 15.0;
//    trajectory_circle(center, radius, update_serve_angle);

    int slice_count = (int)(radius * 10);
    double step = 2.0 * M_PI / (slice_count - 1);

    bool is_down = false;

    double current = 0;
    for (int i = 0; i < slice_count; ++i) {
        double x = center.x + radius * cos(current);
        double y = center.y + radius * sin(current);

        struct Point_t end;
        end.x = x;
        end.y = y;
        double* angles = inverse(end);
        if (angles) {
            update_serve_angle(angles[0], angles[1]);

            if (!is_down) {
                platform_down();
                is_down = true;
            }

            free(angles);
        } else {
            printf("inverse failed\n");
        }

        current += step;
    }

    platform_init();
    return 1;
}

static int doPath(genki_web_request_t *request, genki_web_response_t *response) {

    if (request->method == GET) return -1;

    int contentLength = 0;
    char *cl = request->getHeader(request, "Content-Length");
    if (cl == NULL) {
        return -1;
    }
    contentLength = atoi(cl);

    char buf_str[contentLength + 1];
    int num = request->read(request, buf_str, contentLength);

    buf_str[contentLength] = '\0';
    printf("doPath->%s %d\r\n", buf_str, num);
    // 将buf_str中的数据转换为json格式
    cJSON *json = cJSON_Parse(buf_str);
    if (json == NULL) {
        printf("cJSON_Parse error\r\n");
        return -1;
    }

    /**
     * 解析其中的数据
     * [[-27.9, 42.9], [-15.2, 42.6], [65], [-4.3, 2.3], [6.6, 8.9]
     */

    int count = cJSON_GetArraySize(json);
    // 创建Point_t结构体数组
    struct Point_t points[count];
    for (int i = 0; i < count; ++i) {
        cJSON *item = cJSON_GetArrayItem(json, i);
        if (item == NULL) {
            printf("cJSON_GetArrayItem error\r\n");
            return -1;
        }

        double x = cJSON_GetArrayItem(item, 0)->valuedouble;
        double y = cJSON_GetArrayItem(item, 1)->valuedouble;
        printf("x: %f y: %f\r\n", x, y);

        struct Point_t point;
        point.x = x;
        point.y = y;
        points[i] = point;

        free(item);
    }

    cJSON_Delete(json);

    bool is_up = true;
    platform_up();

    // 遍历points
    for (int i = 0; i < count; ++i) {
        struct Point_t point = points[i];

        if (fabs(point.x - 999.0f) < 0.001f) {
            platform_up();
            is_up = true;
            continue;
        }

        double *angles = inverse(point);
        if (angles) {
            update_serve_angle(angles[0], angles[1]);
            free(angles);
        } else {
            printf("inverse failed x: %f y: %f \r\n", point.x, point.y);
            continue;
        }

        if (is_up) {
            // 确保平台高度控制舵机在落笔的角度
            platform_down();
            is_up = false;
        }
    }
    platform_init();
    return 1;
}

static int doJoints(genki_web_request_t *request, genki_web_response_t *response) {

    if (request->method == GET) return -1;

    char *cl = request->getHeader(request, "Content-Length");
    if (cl == NULL) {
        return -1;
    }
    int contentLength = atoi(cl);

    int recv_len = 0;
    char buf_str[contentLength + 1];

    while (recv_len < contentLength) {
        char buf[128];
        int len = request->read(request, buf, sizeof(buf));
        if (len <= 0) {
            break;
        }
        // 将buf中的数据拷贝到buf_str的后面
        memcpy(&buf_str[recv_len], buf, len);

        recv_len += len;
    }
    buf_str[recv_len] = '\0';
    // 分3次打印buf_str
    printf("doJoints-> %d %s\r\n", recv_len, buf_str);

    // 将buf_str中的数据转换为json格式
    cJSON *json = cJSON_Parse(buf_str);
    if (json == NULL) {
        printf("cJSON_Parse error\r\n");
        return -1;
    }
    // 获取json中的数据
    /**
     * [[-27.91, 82.96], [-15.32, 82.46], [999,999], [-4.13, 82.73], [6.26, 83.95]
     */
    int count = cJSON_GetArraySize(json);
    // 创建Point_t结构体数组
    struct Point_t points[count];
    for (int i = 0; i < count; ++i) {
        cJSON *item = cJSON_GetArrayItem(json, i);
        if (item == NULL) {
            printf("cJSON_GetArrayItem error\r\n");
            return -1;
        }
        double x = cJSON_GetArrayItem(item, 0)->valuedouble;
        double y = cJSON_GetArrayItem(item, 1)->valuedouble;
        printf("x: %f y: %f\r\n", x, y);

        struct Point_t point;
        point.x = x;
        point.y = y;
        points[i] = point;

        free(item);
    }

    cJSON_Delete(json);

    bool is_up = true;
    platform_up();

    // 遍历points
    for (int i = 0; i < count; ++i) {
        struct Point_t point = points[i];
        // 将angle转换为int类型
        double leftAngleDegrees = point.x; // -30 -> 150
        double rightAngleDegrees = 180.0f - point.y; // 30 -> 210

        if (fabs(leftAngleDegrees - 999.0f) < 0.001f) {
            platform_up();
            is_up = true;
            continue;
        }

        update_serve_angle(leftAngleDegrees, rightAngleDegrees);

        if (is_up) {
            // 确保平台高度控制舵机在落笔的角度
            platform_down();
            is_up = false;
        }
    }
    platform_init();

    return 1;
}

static void platform_init() {
    pca9685_servo_set_angle(SERVO_BOTTOM_PORT, 65);
    usleep((int)(200 * 1000));
}

static void platform_shake() {
    pca9685_servo_set_angle(SERVO_BOTTOM_PORT, 65);

    update_serve_angle(80, 180 - 80);
    usleep((int)(300 * 1000));

    update_serve_angle(60, 180 - 60);
    usleep((int)(200 * 1000));
}

static void platform_down() {
    pca9685_servo_set_angle(SERVO_BOTTOM_PORT, 55);
    usleep((int)(250 * 1000));
    pca9685_servo_set_angle(SERVO_BOTTOM_PORT, 45);
    usleep((int)(150 * 1000));
}

static void platform_up() {
    pca9685_servo_set_angle(SERVO_BOTTOM_PORT, 55);
    usleep((int)(200 * 1000));
//    servo_set_angle(SERVO_BOTTOM_PORT, 65);
//    usleep((int)(200 * 1000));
}

static int doInit(genki_web_request_t *request, genki_web_response_t *response) {

    platform_init();            //抬起的舵机65度
    update_serve_angle(90, 90); //左右舵机控制90度

    return 1;
}

genki_web_service_t *newPlotService(void) {
    genki_web_service_t *service = genki_web_newService("PLOTCLOCK");
    service->addFilter(service, "/plot", doHtml);                       //网页来源
    service->addFilter(service, "/plot/init", doInit);                  //安装舵机用的函数doInit
    service->addFilter(service, "/plot/test", doTest);
    service->addFilter(service, "/plot/inverse", doInverse);            //反解
    service->addFilter(service, "/plot/circle", doCircle);              //画圆
    service->addFilter(service, "/plot/path", doPath);
    service->addFilter(service, "/plot/joints", doJoints);

    service->link_name = "小贱钟控制";
    service->link_url = "/plot";

    return service;
}
